package edu.gatech.cc.liam.geometry.linear.convexhull;

import java.util.ArrayList;

import edu.gatech.cc.liam.geometry.linear.Halfspace;
import edu.gatech.cc.liam.geometry.linear.HyperplaneProjection;
import edu.gatech.cc.liam.geometry.linear.NPoint;
import edu.gatech.cc.liam.geometry.linear.PointSet;
import edu.gatech.cc.liam.geometry.linear.Polytope;
import edu.gatech.cc.liam.geometry.linear.programming.InteriorPoint;

public class PolytopeProjectionPipe {
	final private static boolean DEBUG_PRINTS = false;
	
	ArrayList<HyperplaneProjection> theProjectionPipe = new ArrayList<HyperplaneProjection>();
	
//	/**
//	 * reduces the dimensionality of the pointSet if possible
//	 * Then calls QHull to get the extreme points
//	 * Then projects both the points and the hull back up
//	 * @param toProject
//	 * @return
//	 */
//	public static void manifoldHull(PointSet destPoints, Polytope destTope, PointSet inputSet) {
//		PolytopeProjectionPipe projPipe = new PolytopeProjectionPipe();
//		destTope.clear();
//		
//		PointSet projectedPoints = inputSet.clone();
//		
//		Halfspace projectionPlane = projectedPoints.getManifold();
//		while(projectionPlane != null) {
//			destTope.addEqualityConstraint(projectionPlane);
//			HyperplaneProjection theProjection = new HyperplaneProjection(projectionPlane);
//			projPipe.theProjectionPipe.add(theProjection);
//			projectedPoints = theProjection.project(projectedPoints);
//			projectionPlane = projectedPoints.getManifold();
//		}
//		PointSet tmpSet =  new PointSet();
//		Polytope tmpTope = new Polytope();
//		QHull.qConvexHullExact(tmpSet, tmpTope, projectedPoints);
//		destPoints.clear();
//		destPoints.addAll(projPipe.unproject(destPoints));
//		destTope.addAll();
//	}
	
	/**S
	 * 	Reduces the dimensionality of the polytope if possible.
	 *  Then calls QHull to get the extreme points
	 *  Unprojects the points back up to the original dimension.
	 */
	public static PointSet manifoldHull(Polytope toProject) {
		assert toProject.isFeasible(); // assert that the set is feasible

		PolytopeProjectionPipe projPipe = new PolytopeProjectionPipe();
		
		Polytope projectedPoly = projectToManifold(toProject, projPipe);
		PointSet theCEHull;
		if(projectedPoly.getDimensions() > 0) {
			InteriorPoint ip = InteriorPoint.findHalfSpaceInteriorPoint(projectedPoly);
			if(ip.dist < -Preferences.TOLERANCE) {
				return new PointSet();
			} 
			// 3) Compute the intersection of the half-spaces by computing the dual convex hull
			try {
				theCEHull = QHalf.qHalfspaceHull(projectedPoly.toMatrix(), ip);
			} catch (Exception e) {
				System.err.println(e);
				return new PointSet(new NPoint(Double.NaN));
			}
		} else {
			theCEHull = new PointSet();
			theCEHull.add(new NPoint(0));
		}
		if(DEBUG_PRINTS)System.out.println("Hull = " + theCEHull);
		// 3.5) Project the intersectionPoints back into the original dimension
		PointSet unprojectedPoints = projPipe.unproject(theCEHull);
		if(DEBUG_PRINTS)System.out.println("Unprojected Hull = " + unprojectedPoints);
		return unprojectedPoints;
	}

	public static Polytope projectToManifold(Polytope toProject) {
		return projectToManifold(toProject, new PolytopeProjectionPipe());
	}
	public static Polytope projectToManifold(Polytope toProject,
				PolytopeProjectionPipe projPipe) {
		Polytope projectedPoly = toProject.clone();
		Polytope equalityConstraints = projectedPoly.findEqualityConstraints();

		while(!equalityConstraints.isEmpty()) {
			Halfspace projectionPlane = equalityConstraints.remove(0);
			HyperplaneProjection theProjection = new HyperplaneProjection(projectionPlane);
			projPipe.theProjectionPipe.add(theProjection);
			projectedPoly = theProjection.project(projectedPoly);
			equalityConstraints = theProjection.project(equalityConstraints);
		}
		return projectedPoly;
	}
	
	public void addProjection(Halfspace projectionPlane) {
		HyperplaneProjection theProjection = new HyperplaneProjection(projectionPlane);
		theProjectionPipe.add(theProjection);
	}
	
	public Polytope project(Polytope toProject) {
		for(int i=0; i < theProjectionPipe.size(); i++) {
			HyperplaneProjection theProjection = theProjectionPipe.get(i);
			toProject = theProjection.project(toProject);
		}
		return toProject;
	}
	public Halfspace project(Halfspace h) {
		for(int i=0; i < theProjectionPipe.size(); i++) {
			HyperplaneProjection theProjection = theProjectionPipe.get(i);
			h = theProjection.project(h);
		}
		return h;
	}
//	public NPoint project(NPoint p) {
//		for(int i=0; i < theProjectionPipe.size(); i++) {
//			HyperplaneProjection theProjection = theProjectionPipe.get(i);
//			h = theProjection.project(p);
//		}
//		return h;
//	}
	
	public PointSet unproject(PointSet points) {
		for(int i=theProjectionPipe.size()-1; i>=0; i--) {
			HyperplaneProjection theProjection = theProjectionPipe.get(i);
			points = theProjection.unproject(points);
		}
		return points;
	}
	public NPoint unproject(NPoint point) {
		for(int i=theProjectionPipe.size()-1; i>=0; i--) {
			HyperplaneProjection theProjection = theProjectionPipe.get(i);
			point = theProjection.unproject(point);
		}
		return point;
	}
	

	
}
